import robomaster
from robomaster import robot
import time 
from cv2 import cv2


# ep1_robot=robot.Robot()
# ep1_robot.initialize(conn_type='sta',sn='3JKDH6U001R8N3')
# time.sleep(1)
# ep_chassis=ep1_robot.chassis
# speed=50
# time0=time.time()
# seconds=10

class MarkerInfo:

    def __init__(self, x, y, w, h, info):
        self._x = x
        self._y = y
        self._w = w
        self._h = h
        self._info = info

    @property
    def pt1(self):
        return int((self._x - self._w / 2) * 1280), int((self._y - self._h / 2) * 720)

    @property
    def pt2(self):
        return int((self._x + self._w / 2) * 1280), int((self._y + self._h / 2) * 720)

    @property
    def center(self):
        return int(self._x * 1280), int(self._y * 720)

    @property
    def text(self):
        return self._info


markers = []


def on_detect_marker(marker_info):
    number = len(marker_info)
    markers.clear()
    for i in range(0, number):
        x, y, w, h, info = marker_info[i]
        markers.append(MarkerInfo(x, y, w, h, info))
        print("marker:{0} x:{1}, y:{2}, w:{3}, h:{4}".format(info, x, y, w, h))
   


# 巡线时订阅前十个点信息
class PointInfo:

    def __init__(self, x, y, theta, c):
        self._x = x
        self._y = y
        self._theta = theta
        self._c = c

    @property
    def pt(self):
        return int(self._x * 1280), int(self._y * 720)

    @property
    def color(self):
        return 255, 255, 255




class EP1class():

    def __init__(self):

        self.ep1_robot=robot.Robot()
        self.ep1_robot.initialize(conn_type='sta',sn='3JKDH6U001R8N3')
        time.sleep(1)
        self.ep_chassis=self.ep1_robot.chassis
        self.speed=50
        self.time0=time.time()
        self.seconds=10
        self.line = []

    def on_detect_line(self,line_info):
        number = len(line_info)
        self.line.clear()
        line_type = line_info[0]
        #print(line_type)
        if(line_type==1):
            for i in range(1, number):
                x, y, ceta, c = line_info[i]
                self.line.append(PointInfo(x, y, ceta, c))
            x, y, ceta, c = line_info[3]
            update=x-0.5
            # ep_chassis.move(x=pid._integral, y=0, z=0, xy_speed=speed).wait_for_completed()
            self.ep_chassis.drive_wheels(w1=self.speed-update*100, w2=self.speed+update*100, w3=self.speed+update*100, w4=self.speed-update*100)
        if (time.time()-self.time0) > self.seconds:
            self.ep_chassis.drive_wheels(w1=0, w2=0, w3=0, w4=0)
            return

    def flowLine(self,seconds):
        ep_vision = self.ep1_robot.vision
        ep_camera = self.ep1_robot.camera
        ep_camera.start_video_stream(display=False)
        time0=time.time()
        result = ep_vision.sub_detect_info(name="line", color="blue", callback=self.on_detect_line)
        while True:
            img = ep_camera.read_cv2_image()
            for j in range(0, len(self.line)):
                cv2.circle(img, self.line[j].pt, 3, self.line[j].color, -1)
            cv2.imshow("Line", img)
            cv2.waitKey(1)
            if (time.time()-time0) > seconds :
                self.ep_chassis.drive_wheels(w1=0, w2=0, w3=0, w4=0)  
                break 
        cv2.destroyAllWindows()
        result = ep_vision.unsub_detect_info(name="line")
        ep_camera.stop_video_stream()


    def catchfangkuai(self):
        print('EProbot1 ip:{0}'.format(self.ep1_robot.ip))
        print('1号工程兵准备就绪，出发')
        self.ep1_robot.play_audio(filename="./sounds/go.wav").wait_for_completed()
        # 前进 到 仓库
        self.ep_chassis.move(x=0.81, y=0, z=0, xy_speed=0.8).wait_for_completed()
        #右转面对仓库
        self.ep_chassis.move(x=0, y=0, z=-90, z_speed=45).wait_for_completed()
        #向前伸机械臂 机械臂原位坐标x=92 y=42 以此为基础+-
        ep_arm = self.ep1_robot.robotic_arm
        ep_arm.move(x=125, y=-75).wait_for_completed()
        #向前移动到小方块前
        self.ep_chassis.move(x=0.41, y=0, z=0, xy_speed=0.8)
        # ep_chassis.drive_wheels(w1=50, w2=50, w3=50, w4=50)
        time.sleep(1)
        # 机械爪闭合夹取方块
        ep_gripper = self.ep1_robot.gripper
        ep_gripper.close(power=100)
        time.sleep(1)
        ep_gripper.pause()
        time.sleep(1)
        #机械臂回中
        #ep_arm.recenter()
        ep_arm.move(x=-125, y=75).wait_for_completed()
        time.sleep(1)
        #倒退右转回到蓝线上
        self.ep_chassis.move(x=-0.45, y=0, z=0, xy_speed=0.7).wait_for_completed()
        self.ep_chassis.move(x=0, y=0, z=90, z_speed=45).wait_for_completed()
        print('抓取方块完成')

    def main(self,ep1):

        ep_arm = self.ep1_robot.robotic_arm
        ep_gripper = self.ep1_robot.gripper
        while (ep1==0):
            time.sleep(1)
            print('1号工程兵目标还没有确定')
        print('1号工程兵目标已确定为{}号核电站'.format(ep1))
        
        if (ep1==1):
            #语音播报
            self.ep1_robot.play_audio(filename="./sounds/1.wav").wait_for_completed()
            #巡线过第一个右转弯直到1号反应堆左侧
            seconds=10
            self.flowLine(seconds)
            time.sleep(1)

            # #1号反应堆左侧右转
            self.ep_chassis.move(x=0, y=0, z=-88, z_speed=45).wait_for_completed()
            self.ep_chassis.move(x=0.1, y=0, z=0, xy_speed=0.5).wait_for_completed()

            #伸直机械爪
            ep_arm.move(x=115, y=-72).wait_for_completed()
            #放小方块
            ep_gripper.open(power=100)
            time.sleep(1)
            ep_gripper.pause()
            #机械臂回中
            #ep_arm.recenter()
            ep_arm.move(x=-115, y=72).wait_for_completed()
            print('1号工程兵完成混凝土回填')
            #语音播报
            self.ep1_robot.play_audio(filename="ok1.wav").wait_for_completed()


            #夹取红色核燃料  机械爪要到达的位置（194 -17）
            self.ep_chassis.move(x=0, y=0.3, z=0, xy_speed=0.8).wait_for_completed()
            ep_gripper.open(power=100)
            time.sleep(1)
            #伸机械臂
            ep_arm.move(x=125, y=-62).wait_for_completed()
            
            #夹取红色核燃料
            ep_gripper.open(power=100)
            time.sleep(1)
            ep_gripper.close(power=100)
            time.sleep(2)

            #机械臂回中
            ep_arm.move(x=125, y=62).wait_for_completed()
            time.sleep(1)
            ep_arm.move(x=-125, y=-60).wait_for_completed()
            #右转到线上回来放核燃料
            self.ep_chassis.move(x=0, y=0, z=-83, z_speed=45).wait_for_completed()

            
            #巡线到处理站右侧
            seconds=6
            self.flowLine(seconds)

        if (ep1==2):
            #语音播报
            self.ep1_robot.play_audio(filename="./sounds/2.wav").wait_for_completed()
            #巡线过第一个右转弯直到2号反应堆左侧
            seconds=15
            self.flowLine(seconds)

            #2号反应堆左侧右转
            self.ep_chassis.move(x=0, y=0, z=-90, z_speed=45).wait_for_completed()
            self.ep_chassis.move(x=0.1, y=0, z=0, xy_speed=0.5).wait_for_completed()

            #伸直机械爪
            ep_arm.move(x=115, y=-72).wait_for_completed()
            #放小方块
            ep_gripper.open(power=100)
            time.sleep(1)
            ep_gripper.pause()
            #机械臂回中
            #ep_arm.recenter()
            ep_arm.move(x=-115, y=72).wait_for_completed()
            print('1号工程兵完成混凝土回填')
            #语音播报
            self.ep1_robot.play_audio(filename="ok2.wav").wait_for_completed()

            #夹取红色核燃料  机械爪要到达的位置（194 -17）
            self.ep_chassis.move(x=0, y=-0.3, z=0, xy_speed=0.8).wait_for_completed()
            self.ep_chassis.move(x=0.2, y=0, z=45, xy_speed=0.8).wait_for_completed()
            ep_gripper.open(power=100)
            time.sleep(1)
            #伸机械臂
            ep_arm.move(x=125, y=-62).wait_for_completed()
            
            #夹取红色核燃料
            ep_gripper.open(power=100)
            time.sleep(1)
            ep_gripper.close(power=100)
            time.sleep(2)

            #机械臂回中
            ep_arm.move(x=125, y=62).wait_for_completed()
            time.sleep(1)
            ep_arm.move(x=-125, y=-60).wait_for_completed()
            #后退右转回到线上
            self.ep_chassis.move(x=-0.3, y=0, z=0, xy_speed=0.7).wait_for_completed()
            self.ep_chassis.move(x=0, y=0, z=-45, z_speed=45).wait_for_completed()
            #巡线到处理站右侧
            seconds=12
            self.flowLine(seconds)


        #左转
        self.ep_chassis.move(x=0, y=0, z=60, z_speed=45).wait_for_completed()
        #机械爪前伸
        ep_arm.move(x=120, y=-62).wait_for_completed()
        #放下核燃料
        ep_gripper.open(power=100)
        time.sleep(1)
        print('1号工程兵完成核燃料转运任务')
        if (ep1==1):
        # #语音播报
            self.ep1_robot.play_audio(filename="./sounds/done1.wav").wait_for_completed()
        if (ep1==2):
        # #语音播报
            self.ep1_robot.play_audio(filename="./sounds/done2.wav").wait_for_completed()
        #机械臂回中
        ep_arm.move(x=-110, y=62).wait_for_completed()
        #右转
        self.ep_chassis.move(x=0, y=0, z=-70, z_speed=45).wait_for_completed()
        #巡线回到基地
        seconds=6
        self.flowLine(seconds)

        self.ep1_robot.play_audio(filename="./sounds/over.wav").wait_for_completed()
        
        self.ep1_robot.close()
    

